
package nx2.behaviors;

import nx2.utils.*;
import javaclient2.*;

/**
 * Class implementing a Perception Schema (PS), to follow a path based on a set
 * of points. This schema simply evaluates how close the robot is from the current
 * goal point, and defines whether the goal point should be updated or not. The
 * goal point change is can be cyclic (after reaching the last goal point, it
 * will continue to the first point again) or not.
 * @author Alejandro Pustowka
 */
public class NxPSFollowPath extends NxBehavior{

    /**
     * Array of goal points.
     */
    private Vector2D[] pathPoints;

    /**
     * Position2DInterface of the robot.
     */
    private Position2DInterface pos;

    /**
     * Index of the actualPoint.
     */
    private int actualPoint = 0;

    /**
     * Total number of points of the path.
     */
    private int numPoints = 0;

    /**
     * Minimal distance when an update of the actual goal point will ocurr.
     */
    private double minDist = 0.1f;

    /**
     * Defines cyclic behavior.
     */
    private boolean cyclic = false;

    /**
     *
     * @param name Name of the Behavior.
     * @param pathPoints Array of points that define the path.
     * @param pos Position2D Interface of the robot.
     * @param minDist minimal distance for a goal update to ocurr.
     * @param cyclic true or false.
     */
    public NxPSFollowPath(String name, Vector2D[] pathPoints, Position2DInterface pos, double minDist, boolean cyclic)
    {
        super(name);

        this.pathPoints = pathPoints;
        numPoints = pathPoints.length;
        this.pos = pos;

        this.minDist = minDist;
        this.cyclic = cyclic;

    }

    /**
     * Redefine the path points.
     * @param newPath
     */
    public void setNewPath(Vector2D[] newPath)
    {
        this.pathPoints = newPath;
    }

    /**
     * Changes cyclic behavior.
     * @param cyclic
     */
    public void setCyclic(boolean cyclic)
    {
        this.cyclic = cyclic;
    }
    /**
     * Output of this behavior. The actual goal point.
     * @return Vector Output.
     */
    public Vector2D value()
    {
        //first detect distance from the actual position to the point
        Vector2D actualPos = new Vector2D(pos.getData().getPos());

        Vector2D actPoint = pathPoints[actualPoint];

        Vector2D distance = actualPos.sub(actPoint);
        
        if(distance.getMag() <= minDist)
            actualPoint++;

        if(!cyclic)
        {
            if(actualPoint >= numPoints)
                actualPoint = numPoints - 1;
        }
        else
        {
            if(actualPoint >= numPoints)
                actualPoint = 0;
        }
        return new Vector2D(pathPoints[actualPoint]);
    }

}
